/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "cartographer/mapping/2d/submap_2d.h"

#include <cinttypes>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <limits>

#include "Eigen/Geometry"
//#include "absl/memory/memory.h"
#include "cartographer/common/port.h"
#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
// #include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h"
// #include "cartographer/mapping/range_data_inserter_interface.h"
#include "glog/logging.h"

namespace cartographer {
namespace mapping {

// proto::SubmapsOptions2D CreateSubmapsOptions2D(
//     common::LuaParameterDictionary* const parameter_dictionary) {
//   proto::SubmapsOptions2D options;
//   options.set_num_range_data(
//       parameter_dictionary->GetNonNegativeInt("num_range_data"));
//   *options.mutable_grid_options_2d() = CreateGridOptions2D(
//       parameter_dictionary->GetDictionary("grid_options_2d").get());
//   *options.mutable_range_data_inserter_options() =
//       CreateRangeDataInserterOptions(
//           parameter_dictionary->GetDictionary("range_data_inserter").get());

//   bool valid_range_data_inserter_grid_combination = false;
//   const proto::GridOptions2D_GridType& grid_type =
//       options.grid_options_2d().grid_type();
//   const proto::RangeDataInserterOptions_RangeDataInserterType&
//       range_data_inserter_type =
//           options.range_data_inserter_options().range_data_inserter_type();
//   if (grid_type == proto::GridOptions2D::PROBABILITY_GRID &&
//       range_data_inserter_type ==
//           proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D) {
//     valid_range_data_inserter_grid_combination = true;
//   }
//   // if (grid_type == proto::GridOptions2D::TSDF &&
//   //     range_data_inserter_type ==
//   //         proto::RangeDataInserterOptions::TSDF_INSERTER_2D) {
//   //   valid_range_data_inserter_grid_combination = true;
//   // }
//   CHECK(valid_range_data_inserter_grid_combination)
//       << "Invalid combination grid_type " << grid_type
//       << " with range_data_inserter_type " << range_data_inserter_type;
//   CHECK_GT(options.num_range_data(), 0);
//   return options;
// }

Submap2D::Submap2D(const Eigen::Vector2f& origin, std::unique_ptr<Grid2D> grid,
                   ValueConversionTables* conversion_tables)
    : Submap(transform::Rigid3d::Translation(
          Eigen::Vector3d(origin.x(), origin.y(), 0.))),
      conversion_tables_(conversion_tables) {
  grid_ = std::move(grid);
}

Submap2D::Submap2D(const proto::Submap2D& proto,
                   ValueConversionTables* conversion_tables)
    : Submap(transform::ToRigid3(proto.local_pose())),
      conversion_tables_(conversion_tables) {
  if (proto.has_grid()) {
    if (proto.grid().has_probability_grid_2d()) {
      grid_ =
          std::make_unique<ProbabilityGrid>(proto.grid(), conversion_tables_);
    }
    // else if (proto.grid().has_tsdf_2d()) {
    //   grid_ = std::make_unique<TSDF2D>(proto.grid(), conversion_tables_);
    // }
    else {
      LOG(FATAL) << "proto::Submap2D has grid with unknown type.";
    }
  }
  set_num_range_data(proto.num_range_data());
  set_insertion_finished(proto.finished());
}

proto::Submap Submap2D::ToProto(const bool include_grid_data) const {
  proto::Submap proto;
  auto* const submap_2d = proto.mutable_submap_2d();
  *submap_2d->mutable_local_pose() = transform::ToProto(local_pose());
  submap_2d->set_num_range_data(num_range_data());
  submap_2d->set_finished(insertion_finished());
  if (include_grid_data) {
    CHECK(grid_);
    *submap_2d->mutable_grid() = grid_->ToProto();
  }
  return proto;
}

void Submap2D::UpdateFromProto(const proto::Submap& proto) {
  CHECK(proto.has_submap_2d());
  const auto& submap_2d = proto.submap_2d();
  set_num_range_data(submap_2d.num_range_data());
  set_insertion_finished(submap_2d.finished());
  if (proto.submap_2d().has_grid()) {
    if (proto.submap_2d().grid().has_probability_grid_2d()) {
      grid_ = std::make_unique<ProbabilityGrid>(proto.submap_2d().grid(),
                                                conversion_tables_);
    }
    // else if (proto.submap_2d().grid().has_tsdf_2d()) {
    //   grid_ = std::make_unique<TSDF2D>(proto.submap_2d().grid(),
    //                                     conversion_tables_);
    // }
    else {
      LOG(FATAL) << "proto::Submap2D has grid with unknown type.";
    }
  }
}

void Submap2D::ToResponseProto(
    const transform::Rigid3d&,
    proto::SubmapQuery::Response* const response) const {
  if (!grid_) return;
  response->set_submap_version(num_range_data());
  proto::SubmapQuery::Response::SubmapTexture* const texture =
      response->add_textures();
  grid()->DrawToSubmapTexture(texture, local_pose());
  // LOG(ERROR) << "submap pose 2:(" << local_pose() << ").";
}

// void Submap2D::InsertRangeData(
//     const sensor::RangeData& range_data,
//     const RangeDataInserterInterface* range_data_inserter) {
void Submap2D::InsertRangeData(
    const sensor::RangeData& range_data,
    const ProbabilityGridRangeDataInserter2D* range_data_inserter) {
  CHECK(grid_);
  CHECK(!insertion_finished());
  range_data_inserter->Insert(range_data, grid_.get());
  set_num_range_data(num_range_data() + 1);
}

void Submap2D::Finish() {
  CHECK(grid_);
  CHECK(!insertion_finished());
  grid_ = grid_->ComputeCroppedGrid();
  set_insertion_finished(true);
}

// ActiveSubmaps2D::ActiveSubmaps2D(const proto::SubmapsOptions2D& options)
//     : options_(options), range_data_inserter_(CreateRangeDataInserter()) {}
ActiveSubmaps2D::ActiveSubmaps2D()
    : range_data_inserter_(CreateRangeDataInserter()) {}

std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::submaps() const {
  return std::vector<std::shared_ptr<const Submap2D>>(submaps_.begin(),
                                                      submaps_.end());
}

std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
    const sensor::RangeData& range_data) {
  if (submaps_.empty() ||
      submaps_.back()->num_range_data() == kSubmapsNodeCount) {
    AddSubmap(range_data.origin.head<2>());
  }
  for (auto& submap : submaps_) {
    submap->InsertRangeData(range_data, range_data_inserter_.get());
  }
  if (submaps_.front()->num_range_data() == 2 * kSubmapsNodeCount) {
    submaps_.front()->Finish();
  }
  return submaps();
}

// std::unique_ptr<ProbabilityGridRangeDataInserter2D>
// ActiveSubmaps2D::CreateRangeDataInserter() {
//   switch (options_.range_data_inserter_options().range_data_inserter_type())
//   {
//     case proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D:
//       return std::make_unique<ProbabilityGridRangeDataInserter2D>(
//           options_.range_data_inserter_options()
//               .probability_grid_range_data_inserter_options_2d());
//     // case proto::RangeDataInserterOptions::TSDF_INSERTER_2D:
//     //   return std::make_unique<TSDFRangeDataInserter2D>(
//     //       options_.range_data_inserter_options()
//     //           .tsdf_range_data_inserter_options_2d());
//     default:
//       LOG(FATAL) << "Unknown RangeDataInserterType.";
//   }
// }
std::unique_ptr<ProbabilityGridRangeDataInserter2D>
ActiveSubmaps2D::CreateRangeDataInserter() {
  return std::make_unique<ProbabilityGridRangeDataInserter2D>();
}

std::unique_ptr<Grid2D> ActiveSubmaps2D::CreateGrid(
    const Eigen::Vector2f& origin) {
  constexpr int kInitialSubmapSize = 100;
  return std::make_unique<ProbabilityGrid>(
      MapLimits(kResolution,
                origin.cast<double>() + 0.5 * kInitialSubmapSize * kResolution *
                                            Eigen::Vector2d::Ones(),
                CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
      &conversion_tables_);
}

// std::unique_ptr<GridInterface> ActiveSubmaps2D::CreateGrid(
// std::unique_ptr<Grid2D> ActiveSubmaps2D::CreateGrid(
//     const Eigen::Vector2f& origin) {
//   constexpr int kInitialSubmapSize = 100;
//   float resolution = kResolution;
//   switch (options_.grid_options_2d().grid_type()) {
//     case proto::GridOptions2D::PROBABILITY_GRID:
//       return std::make_unique<ProbabilityGrid>(
//           MapLimits(resolution,
//                     origin.cast<double>() + 0.5 * kInitialSubmapSize *
//                                                 resolution *
//                                                 Eigen::Vector2d::Ones(),
//                     CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
//           &conversion_tables_);
//     // case proto::GridOptions2D::TSDF:
//     //   return std::make_unique<TSDF2D>(
//     //       MapLimits(resolution,
//     //                 origin.cast<double>() + 0.5 * kInitialSubmapSize *
//     //                                             resolution *
//     // Eigen::Vector2d::Ones(),
//     //                 CellLimits(kInitialSubmapSize,
//     kInitialSubmapSize)),
//     //       options_.range_data_inserter_options()
//     //           .tsdf_range_data_inserter_options_2d()
//     //           .truncation_distance(),
//     //       options_.range_data_inserter_options()
//     //           .tsdf_range_data_inserter_options_2d()
//     //           .maximum_weight(),
//     //       &conversion_tables_);
//     default:
//       LOG(FATAL) << "Unknown GridType.";
//   }
// }

void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) {
  if (submaps_.size() >= 2) {
    // This will crop the finished Submap before inserting a new Submap to
    // reduce peak memory usage a bit.
    CHECK(submaps_.front()->insertion_finished());
    submaps_.erase(submaps_.begin());
  }
  submaps_.push_back(std::make_unique<Submap2D>(
      origin,
      std::unique_ptr<Grid2D>(
          static_cast<Grid2D*>(CreateGrid(origin).release())),
      &conversion_tables_));
}

}  // namespace mapping
}  // namespace cartographer
